package com.fub.positioning;

import android.util.Log;

import com.fub.guidance.Edge;
import com.fub.guidance.Graph;
import com.fub.guidance.Point;
import com.fub.guidance.Vertex;
import com.fub.util.PreferenceDAO;
import com.fub.util.Util;

public class MapMatcher {

	private Point lastPostion = new Point(0, 0, 0);;
	private PreferenceDAO preferenceDAO;

	/**
	 * Returns the map matched position on graph.
	 * 
	 * @param position
	 *            the position which has to be matched
	 * @param graph
	 *            the routing graph
	 * @param preferenceDao
	 *            the preferences
	 * @return the map matched position on graph
	 */
	public Point getMatchedZoomedPosition(Point position, Graph graph,
			PreferenceDAO preferenceDao) {
		this.preferenceDAO = preferenceDao;
		Point copy = new Point(position.getX(), position.getY(),
				position.getZ());
		float[] matchedPosition = getMatchedPosition(copy, graph);
		this.lastPostion = position;
		return Util.calculateZoomedPosition(matchedPosition, position.getZ(),
				preferenceDao);
	}

	public Point getLastMeasuredZoomedPosition() {
		return Util.calculateZoomedPosition(this.lastPostion,
				this.preferenceDAO);
	}

	private float[] getMatchedPosition(Point measuredPosition, Graph graph) {
		double minDistance = Double.MAX_VALUE;
		float[] matchedPoint = null;

		for (Vertex node : graph.getNodes()) {
			for (Vertex neighbour : node.getAdjacentNodes()) {
				// calculate distance from current point to edge
				float[] tangentIntersection = getPointOnEdge(
						node.getPosition(), neighbour.getPosition(),
						measuredPosition);

				float[] lastTangentIntersection = getPointOnEdge(
						node.getPosition(), neighbour.getPosition(),
						this.lastPostion);

				if (tangentIntersection[0] == -1)
					continue;
				double distance = getDistance(tangentIntersection[0],
						tangentIntersection[1], measuredPosition.getX(),
						measuredPosition.getY());

				double lastDistance = getDistance(lastTangentIntersection[0],
						lastTangentIntersection[1], this.lastPostion.getX(),
						this.lastPostion.getY());

				if (distance + lastDistance < minDistance) {
					minDistance = distance + lastDistance;
					matchedPoint = tangentIntersection;
				}
			}
		}
		return matchedPoint;
	}

	public double getDistance(float x1, float y1, float x2, float y2) {
		return (float) Math.sqrt(Math.pow(x2 - x1, 2) + Math.pow(y2 - y1, 2));
	}

	public float[] getPointOnEdge(Point p1, Point p2, Point p3) {
		float x = -1;
		float y = -1;

		int x1 = p1.getX();
		int x2 = p2.getX();
		int x3 = p3.getX();

		int y1 = p1.getY();
		int y2 = p2.getY();
		int y3 = p3.getY();

		float zaehler = (x3 - x1) * (x2 - x1) + (y3 - y1) * (y2 - y1);
		float nenner = (float) Math.pow(
				Math.sqrt(Math.pow(x2 - x1, 2) + Math.pow(y2 - y1, 2)), 2);
		float u = zaehler / nenner;
		Log.d("Mapmatching", "u " + u);
		if (0 < u && u < 1) {
			x = x1 + u * (x2 - x1);
			y = y1 + u * (y2 - y1);
		}
		float[] result = { x, y };
		return result;
	}

}
